#include "aa_bond_neighborhood.h"
#include "../pdb_index/aa_table.h"
#include "permutations.h"

#include <math.h>
#include <values.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>

//extern struct aa_entry aa_table[];
void calculate_rotation_matrix(gsl_matrix * ret, struct aa_bond_neighborhood* neigh);
void aa_translate(int dx, int dy, int dz, struct aa* aa);

void aa_bond_neighborhood_init(struct aa_bond_neighborhood * neigh, char structure[4], struct aa * center, struct atom * carbon, struct aa neighbors[]) {
  memcpy(&neigh->structure, structure, sizeof(char) * 4);
  memcpy(&neigh->center, center, sizeof(struct aa));
  memcpy(&neigh->backBoneCarbon, carbon, sizeof(struct atom));
  memcpy(&neigh->neighbors, neighbors, sizeof(struct aa) * NEIGHBORS);
  aa_bond_neighborhood_TranslateAndRotate(neigh);
}

void aa_bond_neighborhood_print(struct aa_bond_neighborhood * neigh) {
  int i;
  printf("(aa_bond_neighborhood pdb: %4s ", neigh->structure);
  aa_print(&neigh->center);
  atom_print(&neigh->backBoneCarbon);
  for(i = 0; i < NEIGHBORS; i++) {
    printf("\t");
    aa_print(&neigh->neighbors[i]);
  }
  printf(")\n");
}

double aa_distance (struct aa * aa1, struct aa * aa2) {
  double xd = (double)aa1->x_pos - (double)aa2->x_pos;
  double yd = (double)aa1->y_pos - (double)aa2->y_pos;
  double zd = (double)aa1->z_pos - (double)aa2->z_pos;
  double ret = 0.0;

  double bit_differences = 0.0; 


  int aa1_type_offset = aa_get_table_offset(aa1->type);
  int aa2_type_offset = aa_get_table_offset(aa2->type);

  int i;

  xd*=xd;
  yd*=yd;
  zd*=zd;

  ret += AA_POS_WEIGHT * sqrt(xd+yd+zd);
  
  xd = (double)((aa1->x_dir-aa1->x_pos) - (aa2->x_dir-aa2->x_pos));
  yd = (double)((aa1->y_dir-aa1->y_pos) - (aa2->y_dir-aa2->y_pos));
  zd = (double)((aa1->z_dir-aa1->z_pos) - (aa2->z_dir-aa2->z_pos));

  xd*=xd;
  yd*=yd;
  zd*=zd;

  ret += AA_DIR_WEIGHT * sqrt(xd+yd+zd);
  
  
  if((aa1_type_offset >= 0) && (aa2_type_offset >= 0)) {
    for(i = 0; i < AA_CLASS_COUNT; i++) {
      const struct aa_entry * a1_type = &aa_table[aa1_type_offset];
      const struct aa_entry * a2_type = &aa_table[aa2_type_offset];
      
      if(a1_type->classes[i] != a2_type->classes[i]) {
	bit_differences ++;
      }
    }

  } else if(aa1->type == aa2->type) {
    bit_differences = 0.0;
  } else {
    bit_differences = AA_CLASS_COUNT * 1.0;
  }
  ret += AA_CLASS_WEIGHT * bit_differences;
  
  return ret;
}
/**
 *  This distance function is factorial in the neighborhood size.  I'm
 *  convinced that there is a better way to do this, as matching
 *  points in 3d space is certainly not NP-complete...but it's friday,
 *  and 5! is a pretty small number.  (We'll want bigger neighborhoods
 *  later though.)
 *
 *
 */
double aa_bond_neighborhood_distance(struct aa_bond_neighborhood * n1, struct aa_bond_neighborhood * n2) {
  int perms[NEIGHBORS];
  int i, j;
  double min_dist = MAXDOUBLE; 
  double dist[NEIGHBORS][NEIGHBORS];

  for(i = 0; i < NEIGHBORS; i++) {
    perms[i] = i;
  }
  for(i = 0; i < NEIGHBORS; i++) {
    for(j = 0; j < NEIGHBORS; j++) {       // dist(n1[i], n2[j]) != dist(n1[j], n2[i]), so dist isn't symmetric
      dist[i][j] = aa_distance(&n1->neighbors[i], &n2->neighbors[j]);
    }
  }

  do { 
    double d_sum = aa_distance(&(n1->center), &(n2->center));
  
    for(i = 0; i < NEIGHBORS; i++) {
      d_sum += dist[i][perms[i]];
    }
    //    printf("%f ", d_sum);
    if(d_sum < min_dist) {
      min_dist = d_sum;
    }
  } while(permutation_next(perms, NEIGHBORS));
  //  printf("->%f\n", min_dist);

  return min_dist;

}

//void aa_translate(int dx, int dy, int dz, struct aa* aa);

void aa_bond_neighborhood_TranslateAndRotate(struct aa_bond_neighborhood * n) {
  int dx = -n->center.x_pos;
  int dy = -n->center.y_pos;
  int dz = -n->center.z_pos;
  int i;
  double vector_in[3];
  double vector_out[3];
  gsl_matrix * rotation_matrix = gsl_matrix_alloc(3,3);
  gsl_vector_view vin = gsl_vector_view_array(vector_in, 3);
  gsl_vector_view vout = gsl_vector_view_array(vector_out, 3);

  aa_translate(dx,dy,dz, &n->center);
  n->backBoneCarbon.x +=dx;
  n->backBoneCarbon.y +=dy;
  n->backBoneCarbon.z +=dz;

  calculate_rotation_matrix(rotation_matrix, n);
  /*  printf("Rotation matrix:\n[");
  for(i = 0; i < 3; i++) {
    for(j = 0; j < 3; j++) {
      printf("%f ",gsl_matrix_get(rotation_matrix, i, j));
    }
    printf("\n");
  }
  printf("]\n\n");
  */
  // The center amino acid and backbone carbon were rotated by calculate rotation matrix...

  // Translate and rotate the rest of the neighborhood.
  for(i = 0; i < NEIGHBORS; i++) {
    aa_translate(dx,dy,dz, &n->neighbors[i]);

    vector_in[0] = (double)n->neighbors[i].x_pos;
    vector_in[1] = (double)n->neighbors[i].y_pos;
    vector_in[2] = (double)n->neighbors[i].z_pos;
  
    gsl_blas_dgemv(CblasNoTrans, 1.0, rotation_matrix, &(vin.vector), 0.0, &(vout.vector));

    n->neighbors[i].x_pos = (int)vector_out[0];
    n->neighbors[i].y_pos = (int)vector_out[1];
    n->neighbors[i].z_pos = (int)vector_out[2];

    vector_in[0] = (double)n->neighbors[i].x_dir;
    vector_in[1] = (double)n->neighbors[i].y_dir;
    vector_in[2] = (double)n->neighbors[i].z_dir;
  
    gsl_blas_dgemv(CblasNoTrans, 1.0, rotation_matrix, &(vin.vector), 0.0, &(vout.vector));

    n->neighbors[i].x_dir = (int)vector_out[0];
    n->neighbors[i].y_dir = (int)vector_out[1];
    n->neighbors[i].z_dir = (int)vector_out[2];

  }

  gsl_matrix_free(rotation_matrix);


}

void aa_translate(int dx, int dy, int dz, struct aa* aa) {
  aa->x_pos += dx;
  aa->y_pos += dy;
  aa->z_pos += dz;
  aa->x_dir += dx;
  aa->y_dir += dy;
  aa->z_dir += dz;
}

/**
 *  Given o and a calculate sin and cos with one call to sqrt, and some addition, multiplication and division.
 *  We want to calculate this:
 *  angle = atan(x/y);
 *  c = cos(angle);
 *  s = sin(angle);
 *
 *  But:  s = o/h, c = a/h t = o/a    
 *  (Sine, Cosine and Tangent of the angle formed by a and h, for a right triangle with sides o, a and hypotenuse h)
 *
 *  Now, by the pythagorean thm:
 *
 *  h = sqrt(o*o+a*a);
 *
 *  So, we've avoided three calls to trig functions at the cost of one sqrt and some divisions.
 *
 * c = a/h;
 * s = o/h;
 * 
 * This macro depends on three local variables:
 *   double h, s, c
 * h will contain the hypotenuse of the triangle; c will contain cos, and s will contain sin
 */

#define fast_sin_cos(o, a) h=sqrt(o*o+a*a);c=a/h;s=o/h

/**
   Perform the rotation on the center amino acid and it's backBoneCarbon.
   
   This macro is only defined within calculate_rotation_matrix, and exists in order to
   preserve my sanity only.

   @param rotMat A gsl_matrix_view

   This macro alters the following local variables:

   aa_dirv
   tmpv
   bb_posv


*/

#define rotate_center_bbc(rotMat)							\
  gsl_blas_dgemv(CblasNoTrans, 1.0, &(rotMat.matrix), &(aa_dirv.vector), 0.0, &(tmpv.vector)); 	\
  gsl_vector_memcpy(&(aa_dirv.vector), &(tmpv.vector));					\
  gsl_blas_dgemv(CblasNoTrans, 1.0, &(rotMat.matrix), &(bb_posv.vector), 0.0, &(tmpv.vector));	\
  gsl_vector_memcpy(&(bb_posv.vector), &(tmpv.vector));

/**
   Maintainers:  PLEASE NOTE:

   The following are macros with lots of side effects.  See above:

   rotate_center_bbc
   fast_sin_cos


*/
void calculate_rotation_matrix(gsl_matrix * ret, struct aa_bond_neighborhood * neigh) {

  struct aa * aa = &(neigh->center);

  /*  double x = (double)aa->x_dir;
  double y = (double)aa->y_dir;
  double z = (double)aa->z_dir;
  */
  double aa_dir[3];
  double bb_pos[3];
  
  double xaz[9];
  double zax[9];
  double rbb[9];
  double tmpm_a[9];
  double tmpv_a[3];

  gsl_matrix_view xazm, zaxm, rbbm, tmpm;
  gsl_vector_view aa_dirv, bb_posv, tmpv;

  //  double angle;
  double c;
  double s;
  //  double o, a, 
  double h;

  xazm = gsl_matrix_view_array(xaz, 3, 3);
  zaxm = gsl_matrix_view_array(zax, 3, 3);
  rbbm = gsl_matrix_view_array(rbb, 3, 3);

  tmpm = gsl_matrix_view_array(tmpm_a, 3, 3);
  tmpv = gsl_vector_view_array(tmpv_a, 3);

  aa_dir[0] = (double)aa->x_dir;
  aa_dir[1] = (double)aa->y_dir;
  aa_dir[2] = (double)aa->z_dir;

  aa_dirv = gsl_vector_view_array(aa_dir, 3);

  bb_pos[0] = (double)neigh->backBoneCarbon.x;
  bb_pos[1] = (double)neigh->backBoneCarbon.y;
  bb_pos[2] = (double)neigh->backBoneCarbon.z;
 
  bb_posv = gsl_vector_view_array(bb_pos, 3);

  // Rotate the center amino acid into the x = 0 plane about the z axis.
  // (See derivation in notes..)

  //  angle = atan(x/y);
  //  c = cos(angle);
  //  s = sin(angle);

  //      o = x; a = y;
  // so:  o = aa_dir[0]; a = aa_dir[1];

  fast_sin_cos(aa_dir[0], aa_dir[1]);
  
  xaz[0] =   c; xaz[1] =  -s; xaz[2] = 0.0;
  xaz[3] =   s; xaz[4] =   c; xaz[5] = 0.0;
  xaz[6] = 0.0; xaz[7] = 0.0; xaz[8] = 1.0;

  // Perform the rotation on the center amino acid and it's backBoneCarbon
  rotate_center_bbc(xazm);

  //rot_into_y_about_x

  // Rotate center amino acid into the z = 0 plane about the x axis.
  //  angle = -atan(z/y); = atan(-z/y)
  //  c = cos(angle);
  //  s = sin(angle);

  fast_sin_cos(-aa_dir[2], aa_dir[1]);

  zax[0] = 1.0; zax[1] = 0.0; zax[2] = 0.0;
  zax[3] = 0.0; zax[4] =   c; zax[5] =  -s;
  zax[6] = 0.0; zax[7] =   s; zax[8] =   c;

  rotate_center_bbc(zaxm);

  // The direction vector now lies along the y axis.

  // Rotate the backbone into the z=0 plane about the y axis

  // Need to use the backbone atom, not the bond location here...

  //  angle = atan(z/x);
  //  c = cos(angle);
  //  s = sin(angle);

  fast_sin_cos(bb_pos[2], bb_pos[0]);

  rbb[0] =   c; rbb[1] = 0.0; rbb[2] =   s;
  rbb[3] = 0.0; rbb[4] = 1.0; rbb[5] = 0.0;
  rbb[6] =  -s; rbb[7] = 0.0; rbb[8] =   c;
  

  rotate_center_bbc(rbbm);

  // Now that we have the matrices, and have rotated the center and back bone carbon of the neighorhood,
  // calculate the combined rotation matrix to speed further computations.

  // ret = rbb * (zax * xaz)

  // tmpm = 1.0 * (zax * xaz) + 0.0 * tmpm
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, &(zaxm.matrix), &(xazm.matrix), 0.0, &(tmpm.matrix)); 

  // ret = 1.0 * (rbb * tmpm) * 0.0 * ret
  gsl_blas_dgemm (CblasNoTrans, CblasNoTrans, 1.0, &(rbbm.matrix), &(tmpm.matrix), 0.0, ret); 

  aa->x_dir = (int) aa_dir[0]; 
  aa->y_dir = (int) aa_dir[1]; 
  aa->z_dir = (int) aa_dir[2]; 

  neigh->backBoneCarbon.x = (int) bb_pos[0]; 
  neigh->backBoneCarbon.y = (int) bb_pos[1]; 
  neigh->backBoneCarbon.z = (int) bb_pos[2]; 
 
}

#undef rotate_center_bbc
#undef fast_sin_cos

